import cv2 as cv
import numpy as np
img1 = cv.imread('D:\photo\I7 PLUS\est\IMG_4328.JPG')
img2 = cv.imread('D:\photo\I7 PLUS\est\IMG_4335.JPG')
img1 = cv.resize(img1,None,fx=0.2,fy=0.2)
img2 = cv.resize(img2,None,fx=0.2,fy=0.2)
orb = cv.ORB_create()
kp1 = orb.detect(img1,None)
kp2 = orb.detect(img2,None)
kp1,des1 = orb.compute(img1,kp1)
kp2,des2 = orb.compute(img2,kp2)

bf = cv.BFMatcher(cv.NORM_HAMMING)
match = bf.knnMatch(des1,des2,k=2)
good = []
pt1 = []
pt2 = []

'''• DMatch.distance - 描述符之间的距离。越小越好。
• DMatch.trainIdx - 目标图像中描述符的索引。
• DMatch.queryIdx - 查询图像中描述符的索引。
• DMatch.imgIdx - 目标图像的索引'''
for (m,n) in match:
    if m.distance < 0.8*n.distance:
        good.append(m)
        pt1.append(kp1[m.queryIdx].pt)
        pt2.append(kp2[m.trainIdx].pt)

pts1 = np.int32(pt1)
pts2 = np.int32(pt2)
cameraMatrix = np.array([[1,0,0],[0,1,0],[0,0,1]])
E,mask = cv.findEssentialMat(pts1,pts2,cameraMatrix,cv.FM_RANSAC)
print(E)

U,M,V = np.linalg.svd(E)
R_positive = np.array([[0,-1,0],[1,0,0],[0,0,1]])
t_x = np.dot(U,R_positive)
t_x = np.dot(t_x,M)
t = np.dot(t_x,U.T)

R = np.dot(U,R_positive.T)
R = np.dot(R,M)
R = np.dot(R,V.T)
print('T\n',t,'\n','R\n',R)
